
#include <stdlib.h>
#include "ch.h"
#include "hal.h"

#include "chprintf.h"

#include "myPWM.h"

static PWMConfig pwmcfg3 = {
  100000,                                    /* 0.1MHz PWM clock frequency.   */
  1000,                                    /* 10 ms time period*/

  NULL,
  {                                 /*CH2N and CH3N of Timer 8 are available for PWM. Enable those */
                                                                  /*   disable rest*/
    {PWM_OUTPUT_ACTIVE_HIGH, NULL},
    {PWM_OUTPUT_ACTIVE_HIGH, NULL},
    {PWM_OUTPUT_ACTIVE_HIGH, NULL},
    {PWM_OUTPUT_ACTIVE_HIGH, NULL}
  },
  /* HW dependent part.*/
  0
};

// Speed is -1000..1000
void PWM_set_speed(int MotorA, int MotorB)
{
	if (MotorA == 0) {
		pwmDisableChannel(&PWMD3, 0);
		pwmDisableChannel(&PWMD3, 1);
	} else if (MotorA > 0) {
		pwmEnableChannel(&PWMD3, 1, PWM_PERCENTAGE_TO_WIDTH(&PWMD3, MotorA));
		pwmDisableChannel(&PWMD3, 0);
	} else {
		pwmEnableChannel(&PWMD3, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD3, -MotorA));
		pwmDisableChannel(&PWMD3, 1);
	}
	if (MotorB == 0) {
		pwmDisableChannel(&PWMD3, 2);
		pwmDisableChannel(&PWMD3, 3);
	} else if (MotorA > 0) {
		pwmEnableChannel(&PWMD3, 2, PWM_PERCENTAGE_TO_WIDTH(&PWMD3, MotorB));
		pwmDisableChannel(&PWMD3, 3);
	} else {
		pwmEnableChannel(&PWMD3, 3, PWM_PERCENTAGE_TO_WIDTH(&PWMD3, -MotorB));
		pwmDisableChannel(&PWMD3, 2);
	}

}

void initPWM() {
	pwmStart(&PWMD3, &pwmcfg3);
}

void cmd_pwm(BaseSequentialStream *chp, int argc, char *argv[]) {
  if (argc != 2) {
    chprintf(chp, "Usage: pm #spA #spB\r\n");
    return;
  }
	
  PWM_set_speed(atoi(argv[0]), atoi(argv[1]));
}

